﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Threading;

namespace HubertTestProgram
{
    public partial class MainForm : Form
    {
        #region Fields
        private Thread gUIThread;
        private double gUIThreadSleepInterval;
        private bool running;
        #endregion

        #region Constructors
        public MainForm()
        {
            InitializeComponent();
            this.textBoxPort.Text = "COM10"; // Might be different. Check before running
            this.MoveButton.Enabled = false;
            this.buttonManualControl.Enabled = false;
            this.startVideoViewButton.Enabled = false;
            this.running = false;
        }
        #endregion

        #region Private methods
        private void GUIUpdateLoop()
        {
            while (running)
            {
                pictureBoxInput.Image = Robot.Instance.Body.WebCam.ReadingAsBitmap;
                pictureBoxProcessed.Image = Robot.Instance.Brain.DetectMovingPerson.WebCamProcessedImage;

                if (Robot.Instance.Brain.DetectMovingPerson.PersonDetected)
                {
                    this.labelPersonDetected.Text = "YES";
                    this.labelDirectionToPerson.Text = Robot.Instance.Brain.DetectMovingPerson.DirectionToPerson.ToString();
                }
                else
                {
                    this.labelPersonDetected.Text = "NO";
                }

                this.labelCameraPose.Text = Robot.Instance.Brain.Movement.EstimatedCurrentAbsoluteCameraPose.ToString();
                this.labelPixel.Text = Robot.Instance.Brain.DetectMovingPerson.PixelMiddleOfLargestComponent.ToString();

                Thread.Sleep((int)(gUIThreadSleepInterval * 1000));
            }
        }

        private void InitializeButton_Click(object sender, EventArgs e)
        {
            this.InitializeButton.Enabled = false;

            Robot.Instance.Body = new Body();
            Robot.Instance.Body.SensorPollingThreadSleepInterval = 0.100; // s
            Robot.Instance.Body.WebCam.WebCamDeviceNr = 0;
            Robot.Instance.Body.WebCam.ImageWidth = 320;
            Robot.Instance.Body.WebCam.ImageHeight = 240;
            Robot.Instance.Body.WebCam.FrameRate = 10;
            Robot.Instance.Body.BasicStamp.PortName = this.textBoxPort.Text;
            this.textBoxPort.Enabled = false;
                                                               
            Robot.Instance.Body.BaseRotationServo.ChannelIndex = 0;
            Robot.Instance.Body.ShoulderServo.ChannelIndex = 1;
            Robot.Instance.Body.ElbowServo.ChannelIndex = 2;
            Robot.Instance.Body.HandServo.ChannelIndex = 3;
            Robot.Instance.Body.HeadPanServo.ChannelIndex = 4;
            Robot.Instance.Body.HeadTiltServo.ChannelIndex = 5;

            Robot.Instance.Brain = new Brain();
            Robot.Instance.Brain.BrainStepThreadSleepInterval = 0.010;
            Robot.Instance.Brain.UtilityFunctionUpdateInterval = 0.050;
            Robot.Instance.Brain.Movement.BrainProcessThreadSleepTime = 0.040;
            Robot.Instance.Brain.Movement.Utility = 0.5; // > 0 => Active brain process.
            Robot.Instance.Brain.DetectMovingPerson.BrainProcessThreadSleepTime = 0.050;
            Robot.Instance.Brain.DetectMovingPerson.Utility = 0.5;
            Robot.Instance.Brain.TextToSpeechBrainProcess.BrainProcessThreadSleepTime = 0.200;
            Robot.Instance.Brain.TextToSpeechBrainProcess.Utility = -1;
   
            Robot.Instance.Initialize();

            this.MoveButton.Enabled = true;
            this.buttonManualControl.Enabled = true;
            this.startVideoViewButton.Enabled = true;
            Robot.Instance.Start();
        }

        private void MoveButton_Click(object sender, EventArgs e)
        {
            Robot.Instance.Brain.Movement.ManualControl = false;
            Robot.Instance.Brain.Movement.Utility = 0.5;
        }

        // Called when Form fires FormClosingEvent´. Shut
        private void MainForm_FormClosing(object sender, FormClosingEventArgs e)
        {
            if (Robot.Instance != null)
            {
                Robot.Instance.Stop();
                Robot.Instance.Disconnect();
            }
            Environment.Exit(0);
            //Application.Exit();
        }

        private void startVideoViewButton_Click(object sender, EventArgs e)
        {
            if (!running)
            {
                running = true;
                gUIThreadSleepInterval = 0.080; //0.050
                gUIThread = new Thread(GUIUpdateLoop);
                gUIThread.Start();
                startVideoViewButton.Text = "View Off";
            }
            else
            {
                running = false;
                gUIThread.Join();
                startVideoViewButton.Text = "View On";
            }
        }

        private void buttonManualControl_Click(object sender, EventArgs e)
        {
            Robot.Instance.Brain.Movement.ManualServoBase = (short)this.numericUpDownBase.Value;
            Robot.Instance.Brain.Movement.ManualServoShoulder = (short)this.numericUpDownShoulder.Value;
            Robot.Instance.Brain.Movement.ManualServoElbow = (short)this.numericUpElbow.Value;
            Robot.Instance.Brain.Movement.ManualServoHand = (short)this.numericUpHand.Value;
            Robot.Instance.Brain.Movement.ManualServoPan = (short)this.numericUpDownHeadPan.Value;
            Robot.Instance.Brain.Movement.ManualServoTilt = (short)this.numericUpDownHeadTilt.Value;
            Robot.Instance.Brain.Movement.ManualServoSpeed = (byte)this.numericUpDownSpeed.Value;
            
            Robot.Instance.Brain.Movement.ManualControl = true;
            Robot.Instance.Brain.Movement.Utility = 0.5;

            if (!Robot.Instance.RobotIsStarted)
            {
                Robot.Instance.Start();
            }
        }
        #endregion
    }
}
